import smbus
import time
import logging
import serial
from typing import Optional, Tuple


class PCA9685:
    """PCA9685 PWM控制器类"""
    # 寄存器地址
    __SUBADR1 = 0x02
    __SUBADR2 = 0x03
    __SUBADR3 = 0x04
    __MODE1 = 0x00
    __PRESCALE = 0xFE
    __LED0_ON_L = 0x06
    __LED0_ON_H = 0x07
    __LED0_OFF_L = 0x08
    __LED0_OFF_H = 0x09

    def __init__(self, address=0x40, debug=False):
        self.bus = smbus.SMBus(1)
        self.address = address
        self.debug = debug
        self.logger = logging.getLogger('PCA9685')
        self.setup_pwm()

    def setup_pwm(self):
        """初始化PWM设置"""
        self.write(self.__MODE1, 0x00)
        self.set_pwm_freq(50)
        self.logger.info("PWM初始化完成 (50Hz)")

    def write(self, reg, value):
        """写入I2C寄存器"""
        self.bus.write_byte_data(self.address, reg, value)
        if self.debug:
            self.logger.debug(f"I2C写入 寄存器0x{reg:02X} = 0x{value:02X}")

    def read(self, reg):
        """读取I2C寄存器"""
        result = self.bus.read_byte_data(self.address, reg)
        if self.debug:
            self.logger.debug(f"I2C读取 寄存器0x{reg:02X} = 0x{result:02X}")
        return result

    def set_pwm_freq(self, freq):
        """设置PWM频率"""
        prescaleval = 25000000.0 / 4096.0 / float(freq) - 1.0
        prescale = int(prescaleval + 0.5)
        oldmode = self.read(self.__MODE1)
        self.write(self.__MODE1, (oldmode & 0x7F) | 0x10)
        self.write(self.__PRESCALE, prescale)
        self.write(self.__MODE1, oldmode)
        time.sleep(0.005)
        self.write(self.__MODE1, oldmode | 0x80)

    def set_pwm(self, channel, on, off):
        """设置PWM输出"""
        self.write(self.__LED0_ON_L + 4 * channel, on & 0xFF)
        self.write(self.__LED0_ON_H + 4 * channel, on >> 8)
        self.write(self.__LED0_OFF_L + 4 * channel, off & 0xFF)
        self.write(self.__LED0_OFF_H + 4 * channel, off >> 8)
        self.logger.debug(f"通道{channel} PWM: ON={on}, OFF={off}")

    def set_servo_angle(self, channel, angle):
        """设置舵机角度(0-180度)"""
        angle = max(0, min(180, angle))
        pulse = int(500 + (angle / 180.0) * 2000) * 4096 // 20000
        self.set_pwm(channel, 0, pulse)
        self.logger.info(f"通道{channel} 设置角度: {angle}°")


class PanTiltController:
    def __init__(self, debug=True):
        # 初始化日志
        self.logger = logging.getLogger('PanTiltCtrl')
        self.logger.setLevel(logging.DEBUG if debug else logging.INFO)
        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        ch = logging.StreamHandler()
        ch.setFormatter(formatter)
        self.logger.addHandler(ch)

        # 硬件初始化
        self.pca = PCA9685(debug=debug)
        self.h_servo = 9  # 水平舵机通道
        self.v_servo = 10  # 垂直舵机通道

        # 运动参数
        self.h_range = (20, 160)  # 水平角度范围
        self.v_range = (30, 150)  # 垂直角度范围
        self.center_thresh = 15  # 中心阈值(像素)

        # 控制参数
        self.kp_pan = 1.5  # 水平P值增大
        self.ki_pan = 0.01
        self.kp_tilt = 1.0
        self.ki_tilt = 0.01

        # 状态变量
        self.current_pan = 90
        self.current_tilt = 90
        self.integral_x = 0
        self.integral_y = 0

        # 硬件自检
        self.hardware_test()

    def hardware_test(self):
        """全面的硬件自检"""
        self.logger.info("=== 开始硬件自检 ===")

        # 水平舵机测试
        self.logger.info("水平舵机测试...")
        for angle in [90, 60, 120, 90]:
            self.pca.set_servo_angle(self.h_servo, angle)
            time.sleep(0.5)

        # 垂直舵机测试
        self.logger.info("垂直舵机测试...")
        for angle in [90, 60, 120, 90]:
            self.pca.set_servo_angle(self.v_servo, angle)
            time.sleep(0.5)

        # 双舵机负载测试
        self.logger.info("双舵机负载测试...")
        for angle in range(70, 111, 10):
            self.pca.set_servo_angle(self.h_servo, angle)
            self.pca.set_servo_angle(self.v_servo, 130 - angle)
            time.sleep(0.3)

        # 归中位置
        self.pca.set_servo_angle(self.h_servo, 90)
        self.pca.set_servo_angle(self.v_servo, 90)
        self.logger.info("=== 硬件自检完成 ===")

    def parse_serial_data(self, raw: bytes) -> Optional[Tuple[int, int]]:
        """解析串口数据"""
        try:
            data = raw.decode().strip()
            if data.startswith('$') and data.endswith('#'):
                x, y = map(int, data[1:-1].split(','))
                return x - 160, 120 - y  # 转换为相对中心坐标
        except Exception as e:
            self.logger.error(f"数据解析错误: {e}")
        return None

    def control_servos(self, x: int, y: int):
        """核心控制逻辑"""
        # 水平控制
        if abs(x) > self.center_thresh:
            self.integral_x = max(min(self.integral_x + x, 500), -500)
            adj = x * self.kp_pan + self.integral_x * self.ki_pan
            new_pan = self.current_pan - adj
            new_pan = max(min(new_pan, self.h_range[1]), self.h_range[0])

            if abs(new_pan - self.current_pan) >= 1:
                self.current_pan = new_pan
                self.pca.set_servo_angle(self.h_servo, int(new_pan))
                self.logger.debug(f"水平调整: {new_pan:.1f}° 偏移量: {x}")

        # 垂直控制
        if abs(y) > self.center_thresh:
            self.integral_y = max(min(self.integral_y + y, 500), -500)
            adj = y * self.kp_tilt + self.integral_y * self.ki_tilt
            new_tilt = self.current_tilt + adj
            new_tilt = max(min(new_tilt, self.v_range[1]), self.v_range[0])

            if abs(new_tilt - self.current_tilt) >= 1:
                self.current_tilt = new_tilt
                self.pca.set_servo_angle(self.v_servo, int(new_tilt))
                self.logger.debug(f"垂直调整: {new_tilt:.1f}° 偏移量: {y}")

    def run(self):
        """主控制循环"""
        ser = serial.Serial('/dev/ttyS0', 9600, timeout=1)
        self.logger.info("开始主控制循环...")

        try:
            buffer = b''
            while True:
                # 串口数据处理
                buffer += ser.read(ser.in_waiting)
                while b'#' in buffer:
                    msg, buffer = buffer.split(b'#', 1)
                    if b'$' in msg:
                        data = msg[msg.rfind(b'$'):] + b'#'
                        coords = self.parse_serial_data(data)
                        if coords:
                            self.control_servos(*coords)

                # 空闲时归中
                time.sleep(0.05)

        except KeyboardInterrupt:
            self.logger.info("用户中断")
        finally:
            # 安全关闭
            self.pca.set_servo_angle(self.h_servo, 90)
            self.pca.set_servo_angle(self.v_servo, 90)
            ser.close()
            self.logger.info("系统安全关闭")


if __name__ == "__main__":
    # 初始化日志系统
    logging.basicConfig(
        level=logging.DEBUG,
        format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
        handlers=[
            logging.FileHandler('pan_tilt.log'),
            logging.StreamHandler()
        ]
    )

    # 创建并运行控制器
    controller = PanTiltController(debug=True)
    controller.run()